
import serial
import time
import struct

TTY="/dev/ttyUSB0"


ser=""


def send(format, *args):

        ser.write(struct.pack(format, *args))
    
def cmd(byte):

	send('B', byte)

def init():
	""" 
	Initialisation de la communication
	"""
	ser.setRTS(0)
	time.sleep(0.1)
	ser.setRTS(1)
	time.sleep(2)   
	for i in range(3):
		ser.setRTS(0)
		time.sleep(0.25)
		ser.setRTS(1)
		time.sleep(0.25) 
 
def start():
	"""
	initialisation du roomba
	"""
	cmd(128) #start the sci
	time.sleep(0.1)
	cmd(130) #enable control of roomba
	time.sleep(0.1)

def aspire(dureeSec):
	"""
	Aspiration pendant 2 secondes
	"""
	print "Aspiration pendant %d secondes" %(dureeSec)

	#passage en mode full	
	time.sleep(0.1)
	cmd(132) #full
	
	#demarrer l'aspiration
	time.sleep(0.1)
	send('BB', 138, 2)
	time.sleep(dureeSec)

	#arreter l'aspiration
	send('BB', 138, 0)


def setBaud19200():
	send('BB', 129, 7)#baud: 19200

def setBaud57000():
	send('BB', 129, 10)#baud: 57000


print "Le turtlebot doit etre allume"
print "port utilise: ", TTY




print "Test en 19200"
#essayer de communiquer 19200
ser = serial.Serial(TTY, baudrate=19200, timeout=0.1)
ser.open()
init()
start()

print "Si vous entendez l'aspirateur fonctionner cela confirme qu'il est en 19200 bauds"

#aspirer pour temoigner de la bonne marche
aspire(2)

#Power off
cmd(133)

#fermer le port
ser.close



